#!/usr/bin/env python3
# coding=utf-8

import sys
import copy
import rospy
import numpy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from tf.transformations import quaternion_from_euler

# 主函数
if __name__ == "__main__":
    print("初始化 MoveIt 客户端")
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('mani_plan_exec',anonymous=True)
    # 初始化命令接口
    robot = moveit_commander.RobotCommander()
    # 设置获取场景对象
    scene = moveit_commander.PlanningSceneInterface()
    # 获取规划关节组
    group = moveit_commander.MoveGroupCommander("arm")
    # 延时等待 MoveIt 启动
    # rospy.sleep(5)
    # print("============ 参考坐标系: %s" % group.get_planning_frame())
    # print("============ 当前机器人状态: ")
    # print(robot.get_current_state())
    print("============ 开始规划")
    pose_target = geometry_msgs.msg.Pose()
    # pose_target.header.frame_id = "base_footprint"
    # 手臂末端坐标
    pose_target.position.x = 0.5
    pose_target.position.y = 0.2
    pose_target.position.z = 0.7
    # 手臂末端朝向
    q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), 0.0)
    pose_target.orientation = geometry_msgs.msg.Quaternion(*q)
    group.set_pose_target(pose_target)
    # plan_result = group.plan()
    # print(plan_result[0])
    # print("============ 执行规划")
    go_result = group.go(wait=False)
    print('go_result = ', go_result)